<template>
  <div>
    <h2>ROS 状态: {{ connected ? '✅ 已连接' : '❌ 未连接' }}</h2>
    <p v-if="mapInfo">
      地图信息: {{ mapInfo.width }}x{{ mapInfo.height }}
      分辨率: {{ mapInfo.resolution.toFixed(2) }} m / ({{ mapscale }})/pixel
    </p>

    <canvas ref="canvasRef" class="fixed inset-0 w-full h-full m-0 p-0 z-[-999]"></canvas>

  </div>
</template>

<script setup lang="ts">
import { ref, onMounted, onUnmounted, reactive } from 'vue'
import { Ros, Topic } from 'roslib'

import type { MapMetaData, OccupancyGrid } from "@/interface/nav_msgs/msg"

import type OccupancyGridUpdate from "@/interface/map_msgs/msg/OccupancyGridUpdate"
import { type Pose, Point, Quaternion } from '@/interface/geometry_msgs/msg'
// 两种方法
// import type { OccupancyGridUpdate } from '@/interface/map_msgs/msg'

const connected = ref<boolean>(false)
const canvasRef = ref<HTMLCanvasElement | null>(null)
const mapInfo = ref<MapMetaData | null>(null)

// default unit m/pixel
const mapscale = ref<number>(100)

// 存储上一次的地图数据，用于增量更新
// const previousMap = reactive<{
//   data: number[] | null,
//   info: MapMetaData | null
// }>({
//   data: null,
//   info: null
// })
// 存储上一次的地图数据，用于增量更新
// let previousMapData: number[] | null = null
// let previousMapInfo: MapMetaData | null = null

// const map_pose: Pose = {
//   position: new Point(),
//   orientation: new Quaternion(),
// }

const ros = new Ros({
  url: 'ws://localhost:9090'
})


const map_grid = new Topic<OccupancyGrid>({
  ros: ros,
  name: '/map',
  messageType: 'nav_msgs/msg/OccupancyGrid'
})

const map_grid_update = new Topic<OccupancyGridUpdate>({
  ros: ros,
  name: '/map',
  messageType: 'map_msgs/msg/OccupancyGridUpdate'
})

// 渲染 OccupancyGrid 到 Canvas
const renderOccupancyGrid = (occupancyGrid: OccupancyGrid) => {
  const canvas = canvasRef.value
  if (!canvas) return

  const ctx = canvas.getContext('2d')
  if (!ctx) return

  const { width, height, resolution, origin } = occupancyGrid.info as MapMetaData
  const data = occupancyGrid.data

  // 设置canvas的实际绘制尺寸
  const canvasWidth = canvas.clientWidth
  const canvasHeight = canvas.clientHeight
  canvas.width = canvasWidth
  canvas.height = canvasHeight

  // 清空画布
  ctx.clearRect(0, 0, canvasWidth, canvasHeight)

  // 更新地图信息显示
  mapInfo.value = occupancyGrid.info
  mapInfo.value.resolution = resolution * mapscale.value

  // 计算地图在canvas中心的绘制位置
  const centerX = canvasWidth / 2
  const centerY = canvasHeight / 2

  const offsetx = origin.position.x * mapscale.value
  const offsety = origin.position.y * mapscale.value

  // 计算像素大小
  const pixelSize = resolution * mapscale.value

  console.log('地图绘制信息:', {
    mapSize: `${width}x${height}`,
    canvasSize: `${canvasWidth}x${canvasHeight}`,
    resolution,
    pixelSize,
    center: `${centerX}, ${centerY}`,
    offset: `${offsetx}, ${offsety}`
  })

  // 计算地图绘制的起始位置（以中心为基准）
  for (let x = 0; x < width; x++) {
    for (let y = 0; y < height; y++) {
      const index = x + y * width
      const value = data[index]

      // 根据地图数据值计算颜色
      let color: string
      switch (value) {
        case 0:
          color = 'rgba(255, 255, 255, 0.8)' // 空闲区域 - 增加不透明度
          break;
        case 100:
          color = 'rgba(0, 0, 0, 0.8)' // 障碍物区域 - 增加不透明度
          break;
        default:
          color = 'rgba(128, 128, 128, 0.6)' // 未知区域 - 增加不透明度
          break;
      }

      ctx.fillStyle = color

      // 修复坐标计算 - ROS地图的Y轴是向上的，Canvas的Y轴是向下的
      const canvas_x = centerX + offsetx + x * pixelSize
      const canvas_y = centerY - offsety - (y - 1) * pixelSize

      ctx.fillRect(canvas_x, canvas_y, pixelSize, pixelSize)
    }
  }

  // 保存当前地图数据和信息，用于下次增量更新比较
  // previousMap.data = [...data] // 深拷贝数据数组
  // previousMap.info = { ...occupancyGrid.info } // 深拷贝地图信息
}

ros.on('connection', () => {
  connected.value = true
  console.log('ROS连接成功')
})

ros.on('close', () => {
  connected.value = false
  console.log('ROS连接断开')
})

ros.on('error', (error) => {
  console.error('ROS连接错误:', error)
})

onMounted(() => {
  console.log('开始订阅地图数据...')

  // 订阅地图数据
  map_grid.subscribe((message: OccupancyGrid) => {
    console.log('收到地图数据:', {
      width: message.info.width,
      height: message.info.height,
      resolution: message.info.resolution,
      dataLength: message.data.length
    })
    renderOccupancyGrid(message)
  })

  // 添加错误处理
  map_grid.on('error', (error) => {
    console.error('地图订阅错误:', error)
  })
})

onUnmounted(() => {
  map_grid.unsubscribe()
  map_grid_update.unsubscribe()
})
</script>
